clc
clear
close all

%% inicializace

Ts=0.1;
Tref=10;
Tmax=15;

refPomoc=ones(2,Tref/Ts);

refPomoc(1,(Tref/0.4):(3*Tref/0.4))=20;
refPomoc(2,(Tref/0.2):(Tref/0.1))=80;

for i=1:length(refPomoc);
    ref(2*i-1)=refPomoc(1,i);
    ref(2*i)=refPomoc(2,i);
end


param.ref=ref;

%% simulace1

param.const=0; %koeficient pro matici H

[y,t,out]=simulace(@dualInt,@mpc_dualInt_soft,Tmax/Ts,Ts,zeros(2,1),zeros(2,1),param);

ref=out.ref;

figure
plot(t,y(1,:),t,y(2,:),'g',t,ref(1,:),'r',t,ref(2,:),'k');
grid on;
legend('y1','y2','ref1','ref2');
ylabel('y [-]');
xlabel('t [s]');
title(['soft constranit ',num2str(param.const)]);

%% simulace2
param.const=1;

[y,t,out]=simulace(@dualInt,@mpc_dualInt_soft,Tmax/Ts,Ts,zeros(2,1),zeros(2,1),param);

ref=out.ref;

figure
plot(t,y(1,:),t,y(2,:),'g',t,ref(1,:),'r',t,ref(2,:),'k');
grid on;
legend('y1','y2','ref1','ref2');
ylabel('y [-]');
xlabel('t [s]');
title(['soft constranit ',num2str(param.const)]);

%% simulace3
param.const=10;

[y,t,out]=simulace(@dualInt,@mpc_dualInt_soft,Tmax/Ts,Ts,zeros(2,1),zeros(2,1),param);

ref=out.ref;

figure
plot(t,y(1,:),t,y(2,:),'g',t,ref(1,:),'r',t,ref(2,:),'k');
grid on;
legend('y1','y2','ref1','ref2');
ylabel('y [-]');
xlabel('t [s]');
title(['soft constranit ',num2str(param.const)]);

%% simulace4
param.const=100;

[y,t,out]=simulace(@dualInt,@mpc_dualInt_soft,Tmax/Ts,Ts,zeros(2,1),zeros(2,1),param);

ref=out.ref;

figure
plot(t,y(1,:),t,y(2,:),'g',t,ref(1,:),'r',t,ref(2,:),'k');
grid on;
legend('y1','y2','ref1','ref2');
ylabel('y [-]');
xlabel('t [s]');
title(['soft constranit ',num2str(param.const)]);

%% simulace5
param.const=1000;

[y,t,out]=simulace(@dualInt,@mpc_dualInt_soft,Tmax/Ts,Ts,zeros(2,1),zeros(2,1),param);

ref=out.ref;

figure
plot(t,y(1,:),t,y(2,:),'g',t,ref(1,:),'r',t,ref(2,:),'k');
grid on;
legend('y1','y2','ref1','ref2');
ylabel('y [-]');
xlabel('t [s]');
title(['soft constranit ',num2str(param.const)]);

%% simulace6
param.const=10000;

[y,t,out]=simulace(@dualInt,@mpc_dualInt_soft,Tmax/Ts,Ts,zeros(2,1),zeros(2,1),param);

ref=out.ref;

figure
plot(t,y(1,:),t,y(2,:),'g',t,ref(1,:),'r',t,ref(2,:),'k');
grid on;
legend('y1','y2','ref1','ref2');
ylabel('y [-]');
xlabel('t [s]');
title(['soft constranit ',num2str(param.const)]);